#include "stdafx.h"
#include "CollisionDetection.h"
#include "Robot.h"

// inicjalizacja obiektu statycznego
std::vector<CollisionDetection *> CollisionDetection::models;

void RapidPrepareTri(double toRapid[3][3], Triangle triangle)
{
    /// @todo optymalizacja: vertex moze sie skladac z tablicy[3] zamiast pol x,y,z
    toRapid[0][0] = triangle.vertex[0].x;
    toRapid[0][1] = triangle.vertex[0].y;
    toRapid[0][2] = triangle.vertex[0].z;

    toRapid[1][0] = triangle.vertex[1].x;
    toRapid[1][1] = triangle.vertex[1].y;
    toRapid[1][2] = triangle.vertex[1].z;

    toRapid[2][0] = triangle.vertex[2].x;
    toRapid[2][1] = triangle.vertex[2].y;
    toRapid[2][2] = triangle.vertex[2].z;

}

void CollisionDetection::InitializeCollisionDetectionModel()
{
        unsigned int i;

        for (i=0;i<((StaticObject*)this)->modules.size();i++)
            rapidModels.push_back(RAPID_model());

        CollisionDetection::models.push_back(this);
}

void CollisionDetection::UpdateCollisionDetectionModel(int module)
{

    this->rapidModels[module].BeginModel();
    
    unsigned int i;

    for (i = 0; i < ((StaticObject*)this)->modules[module].triangles.size(); i++)
    {
        double toRapid[3][3];
        RapidPrepareTri(toRapid,((StaticObject*)this)->modules[module].triangles[i]);
        this->rapidModels[module].AddTri(toRapid[0], toRapid[1], toRapid[2], i);
    }

    this->rapidModels[module].EndModel();

}

int CollisionDetection::FindFirstCollisionInAllObjects(unsigned int model)
{
    

    // sprawdzamy kolizje modelu [n] z modelami [n+1,...]
    unsigned int i;
    for (i=model+1;i<models.size();i++)
    {
        if (FindFirstCollisionBetweenModules(models[model],models[i]))
            return 1;

    }

    // rekurencja
    if (model<models.size()-2)
    {
        if (FindFirstCollisionInAllObjects(model+1))
            return 1;
    }

    return 0;

}


int CollisionDetection::FindFirstCollisionBetweenModules(CollisionDetection * model0, CollisionDetection * model1)
{
    unsigned int i,j;
    for (i=0;i<model0->rapidModels.size();i++)
    {
        for (j=0;j<model1->rapidModels.size();j++)
        {
            if (CollisionDetectionInModules(&(model0->rapidModels[i]),&(model1->rapidModels[j])))
            {
                model0->collidedModule=i;
                model1->collidedModule=j;
                return 1;
            }
        }
    }
    model0->collidedModule=-1;
    model1->collidedModule=-1;
    return 0;
}


int CollisionDetection::CollisionDetectionInModules(RAPID_model * model1, RAPID_model * model2)
{

    /// @todo pozbycie sie transformacji przez R i T
    /// @todo lub jesli sie nie pozbedziemy transformacji to mozna chociaz zadeklarowac globalnie te tablice
    double R1[3][3], R2[3][3], T1[3], T2[3];

    R1[0][0] = R1[1][1] = R1[2][2] = 1.0;
    R1[0][1] = R1[1][0] = R1[2][0] = 0.0;
    R1[0][2] = R1[1][2] = R1[2][1] = 0.0;

    R2[0][0] = R2[1][1] = R2[2][2] = 1.0;
    R2[0][1] = R2[1][0] = R2[2][0] = 0.0;
    R2[0][2] = R2[1][2] = R2[2][1] = 0.0;

    T1[0] = 0.0;
    T1[1] = 0.0;
    T1[2] = 0.0;
    T2[0] = 0.0;
    T2[1] = 0.0;
    T2[2] = 0.0;

    // now we can perform a collision query:
    RAPID_Collide(R1, T1, model1, R2, T2, model2, RAPID_FIRST_CONTACT);

    //printf("RAPID: %d/%d\n", RAPID_num_box_tests, RAPID_num_contacts);
    //fflush(stdout);

    if (RAPID_num_contacts)
        return 1;
    else
        return 0;
}
